//
// File: fcNN_LMKF.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 30-Apr-2024 12:50:22
//

// Include Files
#include "fcNN_LMKF.h"
#include "fcNN_LMKF_data.h"
#include "fcNN_LMKF_initialize.h"
#include <algorithm>
#include <cmath>
#include <cstring>

// Variable Definitions
static double x0[14];

static double P0[196];

// Function Definitions
//
// 初始化静态变量
//
// Arguments    : const double Xtest[13]
//                double z
// Return Type  : double
//
double fcNN_LMKF(const double Xtest[13], double z)
{
  static const double Q[196]{
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01};
  static const double d_a[130]{
      0.79417946938725215,   0.33804351177433445,   -0.35319784245276253,
      -0.042894183025405694, -0.91620224396802807,  0.767645442526255,
      0.61439432080352863,   0.62871517246946762,   -0.36145752714602936,
      0.20272428550398472,   -0.75485316106646394,  -0.15215473563154269,
      -0.33772881970539687,  -0.612173756829107,    -0.47376927975083816,
      -0.15819005152192472,  -0.66391171080794731,  0.18097328974800983,
      -0.039507868930553566, 0.40651182314178691,   0.28704397505466511,
      0.35022108092882348,   -0.73222108006786046,  -0.66978308787362428,
      -0.047233732203823542, 0.11471499099413821,   0.24605720214414287,
      -0.17788306624951891,  0.49756813091052871,   0.44522963267353388,
      0.72347867278592,      0.055309851261266751,  -0.31852039417584332,
      -0.76043850363741694,  0.1853367939664633,    0.56383262742574791,
      -0.12861012040991548,  -0.85269785594206171,  -0.37601514407103914,
      -0.71749867536824385,  -0.3159337964809435,   -0.18192797987816023,
      -0.023263737782615861, -0.23072624779334977,  0.3839703156742299,
      -0.014054128106894999, 0.71525495477316725,   0.25430247359018843,
      0.77623242521440483,   -0.52520810262085316,  0.28318360122830671,
      -0.47533209936434939,  0.22164687156020557,   0.32803362652391388,
      -0.84642912552409244,  -0.54044249399268651,  -0.45275523749067853,
      0.31268248299490259,   0.590435469342715,     -0.31560266886322708,
      0.41690739388558112,   0.35586890675458271,   -0.84784305413525785,
      0.30009135566028061,   -0.13556969696138824,  0.84594535789556324,
      -0.8147814866736155,   -0.099450939522255755, -0.11587939684470293,
      -0.083038657801018434, 0.40963614682005561,   -0.36303466311384991,
      0.4431854559745137,    -0.032787713401138285, -0.76756905958310861,
      -0.67475770761857112,  0.35468335001801893,   -0.078865062142103939,
      -0.55554841299282343,  -0.328593646629023,    0.16978186754030281,
      -0.64170715426518521,  0.35958936418758813,   -0.52034042756013232,
      0.61933674160079366,   -0.48566924586160382,  0.42541676890521318,
      -0.66203449383939472,  -0.33560077683603889,  -0.84504582356392288,
      0.08354716680921831,   0.41469936850312389,   0.10131229883158559,
      0.020781710956227779,  0.40205667192975569,   0.33199046631295936,
      0.28623031582710495,   0.27140165045547615,   0.71423798724549037,
      -0.61577355880743867,  0.33228646552738722,   -0.55787949909659174,
      -0.67877591727774322,  0.18663227534462296,   -0.16950390964005896,
      -0.10876357858850048,  0.2582397293490184,    0.5339552265209323,
      -0.23198270917077363,  0.34450740267540858,   -0.10659979400772403,
      0.64518363145667879,   0.5191257990608773,    -0.63039090206736925,
      0.0012236542907487005, 0.12890830157399785,   0.063250396823292657,
      0.74834947364568771,   -0.37341066625284453,  -0.36633236950002923,
      -0.58319205921963524,  0.86126825070421009,   0.21417808555839576,
      -0.0190851386299535,   0.20260568142936861,   0.084494755229442692,
      0.2349468530622798,    0.079413317899298008,  0.36823539632965935,
      0.051651404655941292};
  static const double dv[13]{23.0, 118.5, 64.0, 31.1, 79.3, 69.4, 85.0,
                             47.2, 33.4,  19.1, 24.8, 23.1, 16.1};
  static const double dv1[13]{
      0.0408163265306122, 0.00817494379726139, 0.148148148148148,
      0.099502487562189,  0.0351493848857645,  0.0254129606099111,
      0.0318979266347687, 0.0498753117206983,  0.127388535031847,
      0.135135135135135,  0.099009900990099,   0.186915887850467,
      0.377358490566038};
  static const double b_a[10]{1.0432806971358115,   -1.1864790236108385,
                              -0.75235924679594768, -1.1405635766421163,
                              -1.329181495892958,   -0.19522160365220595,
                              0.15498231362506018,  0.054493622379834031,
                              0.34476905416020565,  0.74288405491999288};
  static const double c_a[10]{-1.5777080963996453,  -1.3851500473142753,
                              0.86189696426528228,  -0.96474055111798518,
                              -0.26420711463667079, 0.1077935295060909,
                              0.562646608151565,    0.93756720730587628,
                              -1.278446233035373,   1.711521222715749};
  double P_[196];
  double c_I[196];
  double H[14];
  double K[14];
  double xp1[13];
  double a;
  double b;
  double d;
  int k;
  signed char b_I[196];
  if (!isInitialized_fcNN_LMKF) {
    fcNN_LMKF_initialize();
  }
  // step_end
  //
  // 运行神经网络
  // MYNEURALNETWORKFUNCTION neural network simulation function.
  //
  //  Auto-generated by MATLAB, 30-Apr-2024 10:28:56.
  //
  //  [y1] = myNeuralNetworkFunction(x1) takes these arguments:
  //    x = Qx13 matrix, input #1
  //  and returns:
  //    y = Qx1 matrix, output #1
  //  where Q is the number of samples.
  //  ===== NEURAL NETWORK CONSTANTS =====
  //  Input 1
  //  Layer 1
  //  Layer 2
  //  ===== SIMULATION ========
  //  Dimensions
  //  samples
  //  Input 1
  //  ===== MODULE FUNCTIONS ========
  //  Map Minimum and Maximum Input Processing Function
  for (k = 0; k < 13; k++) {
    xp1[k] = (Xtest[k] - dv[k]) * dv1[k] + -1.0;
  }
  //  Layer 1
  //  Sigmoid Symmetric Transfer Function
  //  Layer 2
  //  Sigmoid Positive Transfer Function
  //  Output 1
  // step_end
  //
  // 协方差参数
  // step_end
  //
  // 运行线性卡尔曼滤波
  // H矩阵
  a = 0.0;
  for (k = 0; k < 10; k++) {
    d = 0.0;
    for (int i{0}; i < 13; i++) {
      d += d_a[k + 10 * i] * xp1[i];
    }
    a += b_a[k] * (2.0 / (std::exp(-2.0 * (c_a[k] + d)) + 1.0) - 1.0);
  }
  H[0] = 1.0 / (std::exp(-(a + 0.42644519419774979)) + 1.0);
  std::copy(&Xtest[0], &Xtest[13], &H[1]);
  // xs_
  // P_
  for (int i{0}; i < 196; i++) {
    P_[i] = P0[i] + Q[i];
  }
  // K
  a = 0.0;
  for (int i{0}; i < 14; i++) {
    d = 0.0;
    for (int i1{0}; i1 < 14; i1++) {
      d += H[i1] * P_[i1 + 14 * i];
    }
    a += d * H[i];
  }
  b = 1.0 / (a + 0.01);
  // x_
  a = 0.0;
  for (int i{0}; i < 14; i++) {
    d = 0.0;
    for (int i1{0}; i1 < 14; i1++) {
      d += P_[i + 14 * i1] * H[i1];
    }
    K[i] = d * b;
    a += H[i] * x0[i];
  }
  b = z - a;
  for (int i{0}; i < 14; i++) {
    x0[i] += K[i] * b;
  }
  // P
  std::memset(&b_I[0], 0, 196U * sizeof(signed char));
  for (k = 0; k < 14; k++) {
    b_I[k + 14 * k] = 1;
  }
  for (int i{0}; i < 14; i++) {
    for (int i1{0}; i1 < 14; i1++) {
      k = i1 + 14 * i;
      c_I[k] = static_cast<double>(b_I[k]) - K[i1] * H[i];
    }
  }
  // zOUT
  a = 0.0;
  for (int i{0}; i < 14; i++) {
    for (int i1{0}; i1 < 14; i1++) {
      d = 0.0;
      for (k = 0; k < 14; k++) {
        d += c_I[i + 14 * k] * P_[k + 14 * i1];
      }
      P0[i + 14 * i1] = d;
    }
    a += H[i] * x0[i];
  }
  return std::round(a);
  // step_end
  //
  // 更新x0和P0
  // step_end
}

//
// 初始化静态变量
//
// Arguments    : void
// Return Type  : void
//
void fcNN_LMKF_init()
{
  std::memset(&x0[0], 0, 14U * sizeof(double));
  std::memset(&P0[0], 0, 196U * sizeof(double));
  for (int k{0}; k < 14; k++) {
    P0[k + 14 * k] = 1.0;
  }
}

//
// File trailer for fcNN_LMKF.cpp
//
// [EOF]
//
